/***************************************************************************
 *            dc1394cam_driver.cpp
 *  
 *  Wed Oct 21 11:25:00 2009
 *  Copyright  2010  Michael Warren
 *  Queensland University of Technology, CyPhy Lab
 *  <michael.warren@qut.edu.au>
 ****************************************************************************/
#include "dc1394cam_driver.h"

//TODO: Remove this when debugging is complete
#include <iostream>
#include <stdint.h>
#include <algorithm>
using namespace std;

multiCamCapture::multiCamCapture(int num)
{
    captures.resize(num);
#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
    frames.resize(num);
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED
    started = false;
    opened = false;
    debug = false;
    suppress_warnings = false;
}


// Value of open variable
bool multiCamCapture::isOpen() {
    return opened;
}

// Value of open variable
bool multiCamCapture::isStarted() {
    return started;
}

void multiCamCapture::close(void)
{
    for (unsigned int i = 0; i < captures.size(); i++) {
        captures.at(i).close();
    }
    opened = false;
}

// Open all cameras on bus
bool multiCamCapture::open()
{
    bool result = true;
    // Check each camera for a guid set by user. Set up these ones first
    vector<unsigned int> index_list; // Make a list of used indexes
    for (unsigned int i = 0; i < captures.size(); i++) {
        // If user has specified camera with a guid, set it up
        if (captures.at(i).guid) {
            //cout << "multicamcapture::open() guid: " << captures.at(i).guid << endl;
            result = captures.at(i).open();
            if (!result) break; // Something went wrong
            index_list.push_back(captures.at(i).getIndex()); // Add index to our list of used indexes
        }
    }
    // Attempt to open remaining cameras in turn
    //cout << "multicamcapture::open() captures.size(): " << captures.size() << endl;
    //    std::cout << "multicamcapture::open() index_list.size(): " << index_list.size() << endl;
    for (unsigned int i = 0; i < captures.size(); i++) {
        // If not already opened
        if (!captures.at(i).isOpen()) { // If camera hasn't yet been opened
            // Cycle through list of indexes to find a free camera
            for (unsigned int j = 0; j < captures.size(); j++) {
                //cout << "multicamcapture::open() j: " << j << endl;
                // Check to see if index is not already taken
                vector<unsigned int>::iterator it = find(index_list.begin(), index_list.end(), j);
                // If index is not already taken
                if ( index_list.empty() || it == index_list.end()) {
                   // std::cout << "multicamcapture::open() j: " << j << endl;
                    result = captures.at(i).open(j); // Open up the camera
                    index_list.push_back(captures.at(i).getIndex());
                    if (!result) break; // Something went wrong
                }
            }   
        }
    }
    // If everything went well, set open to true
    if (result) { 
        opened = true;
        // If debugging is enabled, write relevent data to disk
        if (debug) {
            // Open up a file with the current time
            char time_string[100];
            time_t curr_time;
            curr_time = time(NULL);
            struct tm *formatted_time;
            formatted_time = localtime(&curr_time);
            size_t len = strftime(time_string, 100, "%y_%m_%d_%H:%M:%S_dc1394cam_driver_debug_output.txt",formatted_time);
            if (!len) return -1;
            std::cout << "dc1394cam_driver Info: Debugging enabled! Writing to file: " << time_string << endl;
            debug_file.open(time_string);
            if (debug_file.fail()) {
                std::cerr << "dc1394cam_driver Error: Error creating output file!" << endl;
            }
            debug_file << "Logging started: " << time_string << "\n";
            debug_file << "multiCamCapture::open(): successfully completed\n";
            debug_file << "Number of cameras: " << captures.size() << "\n";
            for (unsigned int cam_index = 0; cam_index < captures.size(); cam_index++) {
                debug_file << "Camera " << cam_index << ": GUID: " << captures.at(cam_index).getGUID() << "\n";
            }
            debug_file.flush();
        }

    }
    // Attempt to clean up if something went wrong
    else {
        std::cerr << "dc1394cam_driver Error: Could not open cameras properly. Behaviour of subsequent calls"
            << " is undefined." << endl;
        close();
    }
    //cout << "opened variable: " << opened << endl;
    return result;
}

bool multiCamCapture::grabFrames()
{
    // debugging
    if (debug) {
        char* time_string;
        time_t curr_time;
        curr_time = time(NULL);
        struct tm *formatted_time;
        formatted_time = localtime(&curr_time);
        debug_file << formatted_time << " multiCamCapture::grabFrame() called\n";
        debug_file.flush();
    }
    bool result = true;
    // If the process hasn't opened up the cameras, attempt to do it for them quietly
    if (!opened) {
        if (debug) {
            debug_file << "calling multiCamCapture::open()\n";
            debug_file.flush();
         }
        if (!open()) {
            if (debug) {
                debug_file << "multiCamCapture::open() failed, returned non-zero value \n";
                debug_file.flush();
            }
            result = false;
            return result;
        }
    }
    // If cameras haven't been started yet, start them all as 
    // quickly as possible to ensure synchronisaiton
    if (!started) {
        if (debug) {
            debug_file << "multiCamCapture::grabFrame(): capturing not yet started. Attempting to start... \n";
            for (unsigned int cam_index = 0; cam_index < captures.size(); cam_index++) {
                debug_file << "Camera " << cam_index << " Info: \n" <<
                    "    Index: " << captures.at(cam_index).getIndex() << "\n" <<
                    "    Frame Width: " << captures.at(cam_index).getWidth() << "\n" <<
                    "    Frame Height: " << captures.at(cam_index).getHeight() << "\n" <<
                    "    Frame Rate: " << captures.at(cam_index).getFrameRate() << "\n" <<
                    "    Color Mode: " << captures.at(cam_index).getColorMode() << "\n" <<
                    "    ISO Rate: " << captures.at(cam_index).getIsoSpeed() << "\n" <<
                    "    DMA Buffer Size: " << captures.at(cam_index).getDMABufSize() << "\n" << 
                    "    Cleanup Variable: " << captures.at(cam_index).getCleanup() << "\n" <<
                    "    Suppress Warnings Variable: " << captures.at(cam_index).getSuppressWarnings() << "\n" <<
                    "    Color Mode: " << captures.at(cam_index).getColorMode() << "\n";
            }
            debug_file.flush();
        }
        if (!suppress_warnings)
            std::cout << "dc1394cam_driver Info: Will drop frames to ensure synchronisation." << endl;
        for (unsigned int i = 0; i < captures.size(); i++) {
            if (debug) {
                debug_file << "multiCamCapture::grabFrame(): Starting capture for camera " << i << "... ";
                debug_file.flush();
            }
            //Start capturing
            if (!captures.at(i).startCapture()) {
                if (debug) {
                    debug_file << "failed!\n";
                    debug_file.flush();
                }
                result = false;
                return result;
            }
            else {
                if (debug)
                    debug_file << "successful!\n";
            } 
        }
        started = true;
    }
    //cout << "multigrabFrame: started..." << endl;
    uint64_t last_stamp = 0;
    unsigned int max_frames_behind = 0;
    for (unsigned int i = 0; i < captures.size(); i++) {
        // Grab a frame from each camera
        //cout << "multigrabFrame: grabFrame(i)" << endl;
        if (debug)
            debug_file << "multiCamCapture::grabFrame(): Initial grabFrame() for camera " << i << "... ";
        debug_file.flush();
        if (!captures.at(i).grabFrame()) {
            if (debug)
                debug_file << "failed!\n";
            result = false;
            break;
        }
        else {
            if (debug)
                debug_file << "successful!\n";
        }
        // Find the latest timestamp
        if (captures.at(i).getTimeStamp() > last_stamp) {
            last_stamp = captures.at(i).getTimeStamp();
        }
        // Find the most full buffer
        if (captures.at(i).getFramesBehind() > max_frames_behind) {
            max_frames_behind = captures.at(i).getFramesBehind();
        }
        // Check if there are any inconsistencies in the framerate
        uint64_t framePeriod = 1.0/captures.at(i).getFrameRateFloat()*1e6;
        uint64_t timestampDiff = captures.at(i).getTimeStamp() - captures.at(i).getPrevTimeStamp();
        if ((timestampDiff > (framePeriod + framePeriod/3) || // timestamp difference is 33% greater than expected
                timestampDiff < (framePeriod - framePeriod/3)) && // timestamp difference is 33% less than expected
                captures.at(i).getPrevTimeStamp() != 0) { // timestamp difference isn't the first frame
            std::cerr << "dc1394cam_driver Warning: Timestamp inconsistency for camera " << dec << i <<"." << std::endl;
            std::cerr << "Expected timestamp difference: " << framePeriod << " us. Observed timestamp difference: " << timestampDiff << "us." << std::endl;
            std::cerr << "Camera dropped approximately " << timestampDiff/framePeriod << " frames." << std::endl;
            std::cerr << "Camera is currently " << captures.at(i).getFramesBehind() << " frames behind in the buffer of size " << captures.at(i).getDMABufSize() << endl;
        }
    }
    if (debug) {
        debug_file << "multiCamCapture::grabFrame(): After initial capture: \n";
        for (unsigned int i = 0; i < captures.size(); i++) {
            debug_file << "Camera " << i << ":\n" <<
                "    Frames Behind: " << captures.at(i).getFramesBehind() << "\n" <<
                "    TimeStamp: " << captures.at(i).getTimeStamp() << "\n";
        }
    }
    //cout << "multigrabFrame: finished grabbing frames" << endl;
    // If something bad happened, return
    if (!result) return result;
    // Make sure buffers are synchronised
    if (synchroniseBuffers(last_stamp)) {
        if (!suppress_warnings)
            std::cerr << "dc1394cam_driver Warning: Frames out of sync. Corrected by dropping frames." << endl;
    }
    if (debug) {
        debug_file << "multiCamCapture::grabFrame(): After calling synchroniseBuffers: \n";
        for (unsigned int i = 0; i < captures.size(); i++) {
            debug_file << "Camera " << i << ":\n" <<
                "    Frames Behind: " << captures.at(i).getFramesBehind() << "\n" <<
                "    TimeStamp: " << captures.at(i).getTimeStamp() << "\n";
        }
    }
    // Copy frame pointers to frame pointer vector
    for (unsigned int i = 0; i < captures.size(); i++) {
#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
        frames[i] = captures.at(i).retrieveFrame();
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED
#ifdef DC1394CAM_DRIVER_VXL_INSTALLED
        vxlFrames.push_back(captures.at(i).retrieveVxlFrame());
#endif //DC1394CAM_DRIVER_VXL_INSTALLED
    }
    return result;
}

// Synchronises buffers based on their input timestamp. Returns true if it needed to correct
// the synchronisation
bool multiCamCapture::synchroniseBuffers(uint64_t last_stamp) {
    if (debug)
        debug_file << "multiCamCapture::synchroniseBuffers(): Checking timestamps for inconsistencies\n";
    bool outOfSync = false, outOfSyncFlag = false;
    do {
        outOfSync = false;
        // Check timestamps for inconsistencies
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            uint64_t difference = last_stamp - captures.at(cam_index).getTimeStamp();
            uint64_t framePeriod = 1.0/captures.at(cam_index).fps_float*1e6;
            if (difference > framePeriod/3) { // timestamp difference is 33% greater than expected
                if (debug) {
                    debug_file << "multiCamCapture::synchroniseBuffers(): Inconsistency found:\n" <<
                        "    Camera Index: " << cam_index << "\n" <<
                        "    Last Stamp: " << last_stamp << "\n" <<
                        "    Cam " << cam_index << " Time Stamp: " << captures.at(cam_index).getTimeStamp() << "\n" <<
                        "    Difference: " << difference << "\n" <<
                        "    Calling captures.at(" << cam_index << ").grabFrame()\n";
                }
                cout << "multiCamCapture::synchroniseBuffers() Warning: Multicamera frames out of sync. Timestamp difference: " << difference <<"us." << endl;
                outOfSync = true;
                outOfSyncFlag = true;
                if (!captures.at(cam_index).grabFrame()) break;
                // Find the latest timestamp
                //cout << "New timestamp: " << captures.at(cam_index).getTimeStamp() << endl;
                if (debug)
                    debug_file << "    New timestamp: " << captures.at(cam_index).getTimeStamp() << "\n";
                if (captures.at(cam_index).getTimeStamp() > last_stamp) {
                    last_stamp = captures.at(cam_index).getTimeStamp();
                    if (debug)
                        debug_file << "Last stamp is now: " << last_stamp << endl;
                }
            } 
        }
    } while (outOfSync);
    return outOfSyncFlag;
}

#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
// Retrieve all the frames from all the cameras
std::vector<IplImage*> multiCamCapture::retrieveCvFrames()
{
    return frames;
}

// grabFrame and retrieveFrame in one call
std::vector<IplImage*> multiCamCapture::queryCvFrames()
{
    if (grabFrames())
        return frames;
}

// Retrieve frame from a specific camera
IplImage* multiCamCapture::retrieveCvFrame(int i)
{
    return frames.at(i);
}
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED

// Size is the number of camera captures
int multiCamCapture::size(void)
{
    return captures.size();
}


/****************** Set Camera Parameters ***********************/

// Set Firewire Mode
bool multiCamCapture::setIsoSpeed(int speed) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set ISO Speed because cameras are already "
        << "started. Set ISO Speed before attempting to grab frames." << endl;
        result = false;
    }
    else {
        switch (speed) {
            case 100:
            case 200:
            case 400:
            case 800:
            case 1600:
                for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
                    result = captures[cam_index].setIsoSpeed(speed);
                    if (!result) break;
                }
                break;
            default:
                std::cerr << "dc1394cam_driver Error: Cannot set ISO speed of " << speed
                << "MB/s. Firewire supports only 100, 200, 400, 800, 1600 or 3200 MB/s";
                result =  false;
        }
    }
    return result;
}

// Set individual frame width
bool multiCamCapture::setWidth(unsigned int cam_index, int width) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame width because camera is already "
        << "started. Set frame width before attempting to grab frames." << endl;
        result = false;
    }
    else if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        captures[cam_index].setWidth(width);
        result = true;
    }
    return result;
}

// Set frame width for all cameras
bool multiCamCapture::setWidthAllCams(int width) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame width because cameras are already "
        << "started. Set frame width before attempting to grab frames." << endl;
        result = false;
    }
    else {
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            result = captures[cam_index].setWidth(width);
            if (!result) break;
        }
        result = true;
    }
    return result;
}

// Set individual frame height
bool multiCamCapture::setHeight(unsigned int cam_index, int height) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame height because camera is already "
        << "started. Set frame height before attempting to grab frames." << endl;
        result = false;
    }
    else if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setHeight(height);
    }
    return result;
}

// Set frame height for all cameras
bool multiCamCapture::setHeightAllCams(int height) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame height because cameras are already "
        << "started. Set frame height before attempting to grab frames." << endl;
        result = false;
    }
    else {
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            result = captures[cam_index].setHeight(height);
            if (!result) break;
        }
        result = true;
    }
    return result;
}

// Set frame width and height in one call
bool multiCamCapture::setWidthAndHeight(unsigned int cam_index, int width, int height) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame width and height because camera is already "
        << "started. Set frame width and height before attempting to grab frames." << endl;
        result = false;
    }
    else if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setHeight(height);
        if (!result) return result;
        result = captures[cam_index].setWidth(width);
        if (!result) return result;
    }
    return result;
}


// Set frame width and height in one call for all cameras
bool multiCamCapture::setWidthAndHeightAllCams(int width, int height) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame width and height because camera is already "
        << "started. Set frame width and height before attempting to grab frames." << endl;
        result = false;
    }
    else {
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            result = captures[cam_index].setHeight(height);
            if (!result) return result;
            result = captures[cam_index].setWidth(width);
            if (!result) return result;
        }
    }
    return result;
}

// Set framerate
bool multiCamCapture::setFrameRate(std::string framerate) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame rate because cameras are already "
        << "started. Set frame rate before attempting to grab frames." << endl;
        result = false;
    }
    else {
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            result = captures[cam_index].setFrameRate(framerate);
            if (!result) break;
        }
    }
    return result;
}


// Set number of DMA bufs
bool multiCamCapture::setDMABufSize(int num) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set size of DMA buffer because cameras arealready "
        << "started. Set size of DMA buffer before attempting to grab frames." << endl;
        result = false;
    }
    else {
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            result = captures[cam_index].setDMABufSize(num);
            if (!result) break;
        }
        result = true;
    }
    return result;
}

// *************** DC1394 Features **************************

// Set feature value
bool multiCamCapture::setFeature(unsigned int cam_index, std::string featureString, int num) {
    bool result = false;
    if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setFeature(featureString, num);
    }
    return result;
}

// Set absolute feature value
bool multiCamCapture::setFeatureAbs(unsigned int cam_index, std::string featureString, float num) {
    bool result = false;
    if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setFeatureAbs(featureString, num);
    }
    return result;
}

// Set feature to auto
bool multiCamCapture::setFeatureAuto(unsigned int cam_index, std::string featureString) {
    bool result = false;
    if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setFeatureAuto(featureString);
    }
    return result;
}

// Set feature value for all cams
bool multiCamCapture::setFeatureAllCams(std::string featureString, int num) {
    bool result = true;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setFeature(featureString, num);
        if (!result) break;
    }
    return result;
}

// Set absolute feature value
bool multiCamCapture::setFeatureAbsAllCams(std::string featureString, float num) {
    bool result = true;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setFeatureAbs(featureString, num);
        if (!result) break;
    }
    return result;
}

// Set feature to auto
bool multiCamCapture::setFeatureAutoAllCams(std::string featureString) {
    bool result = true;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setFeatureAuto(featureString);
        if (!result) break;
    }
    return result;
}

// ****************************** Point Grey Specific Features ************************** //
// Set feature value
bool multiCamCapture::setPtGreyFeature(unsigned int cam_index, std::string featureString, int num) {
    bool result = false;
    if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setPtGreyFeature(featureString, num);
    }
    return result;
}

// Set absolute feature value
bool multiCamCapture::setPtGreyFeatureAbs(unsigned int cam_index, std::string featureString, float num) {
    bool result = false;
    if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setPtGreyFeatureAbs(featureString, num);
    }
    return result;
}

// Set feature to auto
bool multiCamCapture::setPtGreyFeatureAuto(unsigned int cam_index, std::string featureString) {
    bool result = false;
    if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setPtGreyFeatureAuto(featureString);
    }
    return result;
}

// Set feature value for all cams
bool multiCamCapture::setPtGreyFeatureAllCams(std::string featureString, int num) {
    bool result = true;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setPtGreyFeature(featureString, num);
        if (!result) break;
    }
    return result;
}

// Set absolute feature value
bool multiCamCapture::setPtGreyFeatureAbsAllCams(std::string featureString, float num) {
    bool result = true;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setPtGreyFeatureAbs(featureString, num);
        if (!result) break;
    }
    return result;
}

// Set feature to auto
bool multiCamCapture::setPtGreyFeatureAutoAllCams(std::string featureString) {
    bool result = true;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setPtGreyFeatureAuto(featureString);
        if (!result) break;
    }
    return result;
}



// Set cleanup
bool multiCamCapture::setCleanup(bool value) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set cleanup variable because cameras are already "
        << "started. Set cleanup value before attempting to grab frames." << endl;
        result = false;
    }
    else {
        for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
            result = captures[cam_index].setCleanup(value);
            if (!result) break;
        }
        result = true;
    }
    return result;
}

// Set suppress warnings variable
bool multiCamCapture::setSuppressWarnings(bool value) {
    bool result = false;
    suppress_warnings = value;
    for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
        result = captures[cam_index].setSuppressWarnings(value);
        if (!result) break;
    }
    return result;
}

// Set color mode
bool multiCamCapture::setColorMode(unsigned int cam_index, std::string colorMode) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set color mode because camera is already "
        << "open. Set color mode before attempting to grab frames." << endl;
        result = false;
    }
    else if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        // Make sure color mode is something that driver understands
      if (!isColorValid(colorMode)) {
            result = false;
        }
        else {
            result = captures[cam_index].setColorMode(colorMode);
        }
    }
    return result;
}


// Set color mode for all cams
bool multiCamCapture::setColorModeAllCams(std::string colorMode) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set color mode because camera is already "
        << "open. Set color mode before attempting to grab frames." << endl;
        result = false;
    }
    else {
        // Make sure color mode is something that driver understands
        if (!isColorValid(colorMode)) {
            result = false;
        }
        else {
            for (unsigned int cam_index = 0; cam_index < captures.size(); ++cam_index) {
                result = captures[cam_index].setColorMode(colorMode);
                if (!result) break;
            }
        }
        result = true;
    }
    return result;
}

// Set GUID variable
bool multiCamCapture::setGUID(unsigned int cam_index, uint64_t value) {
    bool result = false;
    if (isOpen()) {
        std::cerr << "dc1394cam_driver Error: Cannot set guid because camera is already "
        << "open. Set guid before calling open()" << endl;
        result = false;
    }    
    else if (cam_index >= captures.size()) {
        std::cerr << "dc1394cam_driver Error: Cam index out of range. Only " << captures.size()
        << " cams available." << endl;
    }
    else {
        result = captures[cam_index].setGUID(value);
    }
    return result;
}

// Set debugging variable
bool multiCamCapture::setDebug(bool value) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set debug variable because cameras are already "
        << "started. Set debug variable before attempting to grab frames." << endl;
        result = false;
    }
    else {
        debug = value;
        result = true;
    }
    return result;
}

/****************** Get Camera Parameters ***********************/ 

// Get Firewire Mode
int multiCamCapture::getIsoSpeed(void) {
    return captures.at(0).getIsoSpeed();
}
// Get frame width
int multiCamCapture::getWidth(unsigned int cam_index) {
    return captures.at(cam_index).getWidth();
}
// Get frame height
int multiCamCapture::getHeight(unsigned int cam_index) {
    return captures.at(cam_index).getHeight();
}
// Get framerate
std::string multiCamCapture::getFrameRate(void) {
    return captures.at(0).getFrameRate();
}
// Get number of DMA bufs
int multiCamCapture::getDMABufSize(void) {
    return captures.at(0).getDMABufSize();
}
// Get cleanup
bool multiCamCapture::getCleanup(void) {
    return captures.at(0).getCleanup();
}
// Get suppress warnings variable
bool multiCamCapture::getSuppressWarnings(void) {
    return captures.at(0).getSuppressWarnings();
}
// Get color mode
std::string multiCamCapture::getColorMode(unsigned int cam_index) {
    return captures.at(cam_index).getColorMode();
}
// Get GUID variable
uint64_t multiCamCapture::getGUID(unsigned int cam_index) {
    return captures.at(cam_index).getGUID();
}

/****************** VXL Functions ***********************/

#ifdef DC1394CAM_DRIVER_VXL_INSTALLED
// Retrieve all the frames from all the cameras
vector<vil_image_view<vxl_byte> >  multiCamCapture::retrieveVxlFrames(void) {
    return vxlFrames;
}
// grabFrame and retrieveFrame in one call
vector<vil_image_view<vxl_byte> > multiCamCapture::queryVxlFrames(void) {
    if (grabFrames())
        return vxlFrames;
}
// Retrieve frame from a specific camera
vil_image_view<vxl_byte> multiCamCapture::retrieveVxlFrame(int i) {
        return vxlFrames.at(i);
}
#endif //DC1394CAM_DRIVER_VXL_INSTALLED

#undef DC1394CAM_DRIVER_VXL_INSTALLED
#undef DC1394CAM_DRIVER_OPENCV_INSTALLED
